/*
自动驾驶开发实践---move base
*/
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include "std_srvs/Empty.h"
#include <sstream>
#include <string>
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "car_control.h"
#include <sensor_msgs/Image.h>
#include "ugv_sdk/scout/scout_base.hpp" //使用小车上的sdk版本
#include <darknet_ros_msgs/BoundingBox.h>
#include <darknet_ros_msgs/BoundingBoxes.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
double goal_point_x[100]; // 记录目标点的x位置
double goal_point_y[100]; // 记录目标点的y位置
double goal_point_z[100]; // 记录目标点的z位置
using namespace westonrobot;
extern ScoutBase scout;
move_base_msgs::MoveBaseGoal cur_goal; // move_base的目标点位置
int count_goal;                        // 导航点数数量
int Number_of_brakes = 10;
int sidewalk_flag = 0; // 未识别
int identify = 0;
int time_delay = 0;
// extern MoveBaseClient ac;
void doMsg(const darknet_ros_msgs::BoundingBoxes::ConstPtr &msgp)
{

    double xmin = msgp->bounding_boxes[0].xmin;
    double ymin = msgp->bounding_boxes[0].ymin;
    double xmax = msgp->bounding_boxes[0].xmax;
    double ymax = msgp->bounding_boxes[0].ymax;
    double probability = msgp->bounding_boxes[0].probability;
    std::string temp = msgp->bounding_boxes[0].Class;
    // ROS_INFO("我订阅: %s", temp.c_str());

    if (identify == 0 && sidewalk_flag == 0 && temp == "sidewalk") // 人行道
    {
        sidewalk_flag = 1;
        identify = 1;
        ROS_INFO("我订阅: %s", temp.c_str());

        // scout.SetMotionCommand(0.4, 0.3);
        for (Number_of_brakes = 0; Number_of_brakes < 3; Number_of_brakes++)
        {
            scout.SetMotionCommand(0.2, 0.0);
            ros::Duration(1.0).sleep();
            scout.SetMotionCommand(0.0, 0.0);
            ros::Duration(1.0).sleep();
        }
    }
    else if (temp == "nleft") // 紧急左转
    {
        /* code */
    }
    else if (temp == "redlight") // 红灯
    {
        for (long i = 0; i < 100; i++)
        {
            scout.SetMotionCommand(0.0, 0.0); // 停止运动
        }
    }
    else if (temp == "greenlight") // 绿灯
    {
        return; // 继续行驶
    }
    else if (temp == "Ultraman") // 奥特曼
    {
        /* code */
    }
    else if (temp == "Godzilla") // 哥斯拉
    {
        /* code */
    }
    // ROS_INFO("我听见:%s",(*msg_p).data.c_str());
}
void callback1(const ros::TimerEvent &)
{
    time_delay++;
    ROS_INFO("Callback 1 triggered");
    if (sidewalk_flag == 1)
    {
        // ac.cancelAllGoals();
        ROS_INFO_STREAM("进来了!  ");

        // if (time_delay < 10)
        //     scout.SetMotionCommand(0.8, 0.0);
        // else if (time_delay > 10 && time_delay < 20)
        //     scout.SetMotionCommand(0.0, 0.0);
        // else if (time_delay > 20 && time_delay < 30)
        //     scout.SetMotionCommand(0.8, 0.0);
        // else if (time_delay > 30 && time_delay < 40)
        //     scout.SetMotionCommand(0.0, 0.0);
        // else if (time_delay > 20 && time_delay < 30)
        //     scout.SetMotionCommand(0.8, 0.0);

        // time_delay = 0;
        //  identify = 0
        //  Number_of_brakes = 0;
        sidewalk_flag = 0;
        //       ac.sendGoal(cur_goal);
        // 导航到下一个点，将identify=0
    }
    if (time_delay > 100)
        time_delay = 0;
}
int main(int argc, char **argv)
{

    ros::init(argc, argv, "multi_point_node");
    ros::NodeHandle n; // 定义节点进程句柄
    ros::Subscriber sub = n.subscribe("/darknet_ros/bounding_boxes", 10, doMsg);
    ros::Timer timer1 = n.createTimer(ros::Duration(0.3), callback1);

    setlocale(LC_CTYPE, "zh_CN.utf8"); // 设置能输出中文
    base_init();                       // 底盘链接初始化
    get_goal();                        // 从启动文件中读取地图各个目标点的数据
    /* 创建AsyncSpinner */
    ros::AsyncSpinner spinner(2); // 这里的参数表示使用一个线程
                                  //    ros::spin();            // 进入ROS循环
    ros::Rate loop_rate(100);     // 设置循环时间为 10ms一次
    spinner.start();
    while (ros::ok())
    {
        // ros::spin();
        car_move(); // 通过识别目标控制小车的移动
        loop_rate.sleep();
        // ros::spinOnce();
    }
    spinner.stop();
    return 0;
}
